﻿#ifndef SLAMROUTEPLANNING_H
#define SLAMROUTEPLANNING_H

#include <QDir>
#include <QFile>
#include <QList>
#include <QObject>

#include "dijkstra.hpp"
#include "param.h"
#include "point.h"
#include "station.h"
#include "utility.h"

class SlamRoutePlanning : public QObject {
  Q_OBJECT
 public:
  explicit SlamRoutePlanning(QObject* parent = nullptr);

  enum FunModel {
    STANDBY = 0,
    ALLOW_TRANSMIT = 1,
  };

  enum StationFunModel {
    GOON = 0,
    FORK_UP = 1,
    FORK_DOWN_IF_EMPTY = 2,
  };

  enum RouteType {
    NORMAL = 0,
    ENTER = 1,
    EXIT = 2,
  };

  struct StationModel {
    bool right_angle_turn = true;
    RouteType route_type = NORMAL;
    bool isforward = true;
    QList<Station> stations;
    StationFunModel station_fun = GOON;
  };

  struct SlamModel {
    int num = 0;
    int first_station_id = 0;
    int last_station_id = 0;
    FunModel fun_model = STANDBY;
    QList<StationModel> station_model;
  };

  struct ObsModel {
    int slam_point_id = 0;
    Point obs_point;
    double slam_rad = 0.0;
    double errorvalue;
    int distance = 0;
    int front = 3;
    int back = 3;
    int left = 1;
    int right = 1;
  };

  struct SlamObsModel {
    int start = 0;
    int end = 0;
    int front = 3;
    int back = 3;
  };

  struct chargeIdAndIp {
    int id = 0;
    QString ip = 0;
  };
  struct anglejudgmentid {
    int id = 0;
  };

  struct MachineMsg {
    int id = 0;
    QString msg = "空";
  };

  struct ManInitPose {
    int id = 0;
    Point point;
  };

  struct goalYaw {
    int id = 0;
    double yaw;
  };

  struct backpoint {
      int id;
      int num =0;
      double speed = 0;
  };

  struct stopBarrier {
    int fun = 0;
    int start = 0;
    int end = 0;
    int dis = 0;
  };

  void allFileReset();
  void creatSlamRouteMsg();
  void routeInit();
  void routePrint();
  // void redoRouteInit();
  // void redoRoutePrint();
  void obsInit();
  void obsPrint();
  void SlamObsInit();
  void SlamObsPrint();
  void chargeIdAndIpInit();
  void chargeIdAndIpPrint();
  void anglejudgmentInit();
  void anglejudgmentPrint();
  void machineMsgInit();
  void machineMsgPrint();
  void readSlamRouteFile(const int& num);
  void manInitPoseInit();
  void manInitPosePrint();
  void goalyawInit();
  void goalyawPrint();
  void backpointInit();
  void backpointPrint();
  void stopBarrierInit();
  void stopBarrierPrint();

  // QList< StationModel > getRedoStationMode( const int num );
  QList<StationModel> getStationModel(const int& num);
  QList<StationModel> caluStatuonModel(const int& start, const int& goal, RouteType route_type);
  FunModel getFunMode(const int& num);
  QString getFunModeMsg(const int& num);
  QString getMachineMsg(const int& num);

  bool slamPointInit();
  bool isSlamRoutes(const int& num);
  bool isGoalVaild(const int& id);
  bool isIdVaild(const int& id);
  bool isNeedChangeObs(const Point& curPoint, int* front, int* back, bool* Lobs, bool* Robs);
  bool isNeedChangeSlamObs(const int& start, const int& end, int* front, int* back);
  bool isChargeNum(const int& id);
  bool isAnglejudgment(const int& id);
  bool getManInitPosePoint(const int& num, Point& curPoint);
  bool getgoalYaw(const int& num, double& yaw);

  bool getbackpoint(const int &id ,int& num, double& speed);

  bool isNeedStopBarrier(int fun, const int& start, const int& end, const double& start2targetdis, const double& cur2targetdis);
  int getSlamRouteSum() const;
  int getSlamPointId(const Point& curPoint);
  bool findResentSlamPoint(const Point& curPoint, ID& id, int& min_dist);
  bool getSlamPointValue(const int& id, Point& curPoint);
  QString getChargeIp(const int& id);
  int getSlamRouteFirstId(const int& num);
  int getSlamRouteLastId(const int& num);

  QList<int> getSlamRoute(const int& lastId);
  static void getIdAndRouteType(QString str, ID& id, RouteType& type);

 private:
  QList<Station> slam_point;
  Map map, map_enter, map_exit;
  QList<SlamModel> slam_routes, reset_slam_routes;  // redo_slam_routes
  QList<ObsModel> obs_model;
  QList<chargeIdAndIp> charge_id_and_ip;
  QList<anglejudgmentid> anglejudgment_id;
  QList<MachineMsg> machineMsg;
  QList<goalYaw> goal_yaw;
   QList<backpoint> back_point;
  QList<ManInitPose> man_init_pose;
  QList<stopBarrier> stop_barrier;
  QList<SlamObsModel> slam_obs_mode;
};

#endif  // SLAMROUTEPLANNING_H
